#include "YKThreadProcImg.h"

#include "itkFFTNormalizedCorrelationImageFilter.h"
//#include "itkRegionOfInterestImageFilter.h"
#include "itkImageKernelOperator.h"
#include "itkRescaleIntensityImageFilter.h"
#include "itkImageFileWriter.h"
#include "itkMinimumMaximumImageCalculator.h"
#include "itkCropImageFilter.h"

using namespace std;


bool ComparePixInfoProc(PIXINFO first, PIXINFO second)
{
	//return first.val < second.val;//descending
	return first.val > second.val;//Ascending
}


//iCrntIDx: last index, iNumProj: number of projection images to be processed.
//If iCrntIdx =10, iNumProj:5 ==> IDX 6,7,8,9,10 images will be copied to this Thread

YKThreadProcImg::YKThreadProcImg(DPGMTracking* pParent, int iCrntIdx, float fMVGanAngle, float fPanelShiftX, float fPanelShiftY, bool bKVOn, bool bMVOn, int iNumProj)
{  
  m_pParent = pParent;
  m_iCrntIdx = iCrntIdx;
  m_iNumProj=  iNumProj; //number of proj for processing

  m_fMVGanAngle = fMVGanAngle;
  m_fPanelShiftX = fPanelShiftX;
  m_fPanelShiftY = fPanelShiftY;
  m_bKVOn = bKVOn;
  m_bMVOn = bMVOn;

  //memory allocated here
  /*if (m_iCrntIdx >= 0 && m_iNumProj >= 1)
  {
  m_pArrData = new unsigned short* [m_iNumProj];

  int cntPix = DEFAULT_WIDTH*DEFAULT_HEIGHT;

  for (int i = 0 ; i<m_iNumProj ; i++)
  {
  m_pArrData[i] = new unsigned short [cntPix];
  }
  }
  else
  {
  m_pArrData = NULL;
  }*/
  //m_pArrData = NULL

  m_pYKImageProc = new YK16GrayImage();
  m_pYKImageROIProc = new YK16GrayImage(100,100);
  m_spSrcImage = UnsignedShortImageType::New();

  int templtSizeX = 31;
  int templtSizeY = 31;

  //m_fDefaultSpacingX = 0.4;
  //m_fDefaultSpacingY = 0.4;
  
  CreateTemplate(31, 31, 0.4, 0.4); //fill m_spCCTemplate
}

YKThreadProcImg::~YKThreadProcImg()
{

  //Release memory 

	/*if (m_pArrData != NULL)
	{
	for (int i = 0 ; i<m_iNumProj ; i++)
	{
	delete [] m_pArrData[i];
	}
	delete [] m_pArrData;
	}*/
	delete m_pYKImageProc;
}


//current implementation: create a 2D template
void YKThreadProcImg::CreateTemplate(int sizeX, int sizeY, float spacingX, float spacingY)
{
	m_spCCTemplate = UnsignedShortImageType::New();

	UnsignedShortImageType::IndexType start;
	start.Fill(0);
	UnsignedShortImageType::SizeType size;
	size[0] = sizeX;
	size[1] = sizeY;
	//size.Fill(61);
	UnsignedShortImageType::RegionType region(start, size);

	UnsignedShortImageType::PointType origin;
	origin.Fill(0.0);

	UnsignedShortImageType::SpacingType spacing;
	//spacing.Fill(0.4);
	spacing[0] = spacingX;
	spacing[1] = spacingY;

	m_spCCTemplate->SetRegions(region);
	m_spCCTemplate->SetOrigin(origin);
	m_spCCTemplate->SetSpacing(spacing);
	m_spCCTemplate->Allocate();
	m_spCCTemplate->FillBuffer(1);

//1 mm -> mag 1.536 = 1.5 mm
	// 1.5 / spacing ~= 4 px
	float mag = 1.536;
	float physThick = 1.0;	

	itk::ImageRegionIterator<UnsignedShortImageType> imageIterator(m_spCCTemplate, region);
	UnsignedShortImageType::IndexValueType squareSize = std::ceil(mag/physThick / spacingX);
	UnsignedShortImageType::IndexValueType centerIdx = qRound(m_spCCTemplate->GetLargestPossibleRegion().GetSize()[0] / 2.0);

	while (!imageIterator.IsAtEnd())
	{
		if (imageIterator.GetIndex()[0] > centerIdx - qRound(squareSize / 2.0) && imageIterator.GetIndex()[0] <  centerIdx + qRound(squareSize / 2.0) &&
			imageIterator.GetIndex()[1] > centerIdx - qRound(squareSize / 2.0) && imageIterator.GetIndex()[1] < centerIdx + qRound(squareSize / 2.0))
		{
			imageIterator.Set(0);
		}
		++imageIterator;
	}

	/*typedef itk::ImageFileWriter<UnsignedShortImageType> writerType;
	writerType::Pointer writer = writerType::New();

	writer->SetInput(m_spCCTemplate);
	writer->SetFileName("D:\\Test2D.mha");
	writer->SetUseCompression(true);

	writer->Update();
*/
}


void YKThreadProcImg::run()//called by thread.start()
{ 
//In current version, don't do anything if KVON flag is false
	if (!m_bKVOn)
	{
		cout << "Thread Proc ID " << m_iCrntIdx << " is skipped due to the KVON flag" << endl;
		exec();//event roop Thread is still alive To quit thread, call exit()
		return;
	}

  //put while loop or something here
  //This function will be called after Thread.Start();
	QTime procedureStart = QTime::currentTime();

	//No while loop is needed for this Thread.
	m_pParent->m_mutex.lock(); //maybe not needed just for copying
	CopyDataFromMain(); //deep copying of pYKProc and other parameters
	m_pParent->m_mutex.unlock();

	m_pYKImageProc->m_fMVGantryAngle = m_fMVGanAngle;
	m_pYKImageProc->m_fPanelOffsetX = m_fPanelShiftX;
	m_pYKImageProc->m_fPanelOffsetY = m_fPanelShiftY;
	m_pYKImageProc->m_bKVOn = m_bKVOn;
	m_pYKImageProc->m_bMVOn = m_bMVOn;

	m_pYKImageProc->m_strTimeStamp = procedureStart.toString("hhmmsszzz");

	vector<VEC3D>::iterator it;

	QPoint output2D;		
	m_pYKImageProc->ClearMarkerPosRef();//not needed

	for (it = m_vRefMarkerPosDCMProc.begin(); it != m_vRefMarkerPosDCMProc.end(); it++)
	{
		output2D = Get2DPosFromDCM3D((*it).fX + m_fOffsetLat, (*it).fY + m_fOffsetVert, (*it).fZ + m_fOffsetSI,
			m_pYKImageProc->m_fMVGantryAngle, m_pYKImageProc->m_fPanelOffsetX, m_pYKImageProc->m_fPanelOffsetY,
			ELEKTA_SAD, ELEKTA_SID, m_pYKImageProc->m_iWidth, m_pYKImageProc->m_iHeight, m_pYKImageProc->m_fSpacingX, m_pYKImageProc->m_fSpacingY);

		//What if the ref 2D pos is out of range? dealt with in FindMarkerInROI_B
		//Out if range flag: m_bvRefOutOfRanges
		m_pYKImageProc->AddMarkerPosRef(output2D.x(), output2D.y()); //in an order of the marker 1, 2, 3 (superior mareker= 1st marker)
	}

	//RESET ROI

	int marginLt = 0;
	int marginTop = 0;
	int marginRt = 0;
	int marginBottom = 0;

	if (m_pYKImageProc->m_vMarkerRef.empty())
	{
		//use original ROI of the YKimage
	}
	else
	{
		QStringList strListMargin = m_strMargin.split(",");

		if (strListMargin.length() < 4)
		{
			marginLt = 100;
			marginTop = 200;
			marginRt = 100;
			marginBottom = 200;
		}
		else
		{
			marginLt = strListMargin.at(0).toInt();
			marginTop = strListMargin.at(1).toInt();
			marginRt = strListMargin.at(2).toInt();
			marginBottom = strListMargin.at(3).toInt();
		}

		ResetROIExt(m_pYKImageProc, marginLt, marginTop, marginRt, marginBottom);
	}

	ImgProcessingForROI(m_pYKImageProc, m_pYKImageROIProc, m_paramset); //includes every image processing
	
	vector<PIXINFO> vOutputWithCC;	
	FindMarkerInROI_A(m_pYKImageProc, m_pYKImageROIProc, vOutputWithCC, m_fThreMultiplySD, m_iSameMarkerMargin);
	
	double meanDistanceError = FindMarkerInROI_B(m_pYKImageProc, m_pYKImageROIProc, vOutputWithCC,
		m_iCandidate, m_pYKImageProc->m_iNumOfTrackMarker, m_motionW_X, m_motionW_Y, m_CC_weighting, m_vPrevFrameMarkerPosProc);
	
	/***********************Write image in main GUI ************************/
	m_pParent->m_mutex.lock(); //maybe not needed just for copying
	
	//Main needs only
	//1) tracking data: included in m_pYKimage. m_vMarkerPos

	m_pParent->m_iLastProcessedIdx = m_iCrntIdx;
	m_pParent->m_arrYKImageROI[m_iCrntIdx].CloneImage(*m_pYKImageROIProc);	
	m_pYKImageProc->m_iProcessingElapsed = procedureStart.elapsed();
	m_pParent->m_arrYKImage[m_iCrntIdx].UpdateTrackingData(m_pYKImageProc);
	//2) for display / log m_vmarkerPosRef, m_YKROI, and m_iLastProcIndex ...

	m_pParent->m_mutex.unlock();

	cout << "Thread Proc ID " << m_iCrntIdx << " was successfully done" << endl;
	exec();//event roop Thread is still alive To quit thread, call exit()
  //quit thread 
}

void YKThreadProcImg::CopyDataFromMain()
{	
	//1) allocate memory
	if (m_pParent == NULL)
		return;
	if (m_pParent->m_iCntMaxImg < 1)
		return;

	//1) Copying Data from parent (main Thread)
	//int firstIdx = m_iCrntIdx - m_iNumProj + 1; // 10 - 5 = 5  
	//int mainDataIdx = 0;

	cout << "Proc img idx= " << m_iCrntIdx << endl;
/*
	for (int i = 0; i < m_iNumProj; i++)
	{
		mainDataIdx = firstIdx + i;
		if (mainDataIdx >= 0)
			m_pYKImageProc->CloneImage(m_pParent->m_arrYKImage[mainDataIdx]);		

	}*/

	m_pYKImageProc->CloneImage(m_pParent->m_arrYKImage[m_iCrntIdx]);
	cout << "Proc Img Width ROI " << m_pYKImageProc->m_rtROI.width() << endl;

	//Copy GUI data of main
	m_fOffsetLat = m_pParent->ui.lineEditCouchLat->text().toDouble();
	m_fOffsetVert = m_pParent->ui.lineEditCouchVert->text().toDouble();
	m_fOffsetSI = m_pParent->ui.lineEditCouchSI->text().toDouble();

	//m_iDispMarker = m_pParent->ui.lineEditNumOfMarkerDisp->text().toInt();
	//m_iNumOfTrackMarker = m_pParent->ui.lineEditNumOfMarkerTrack->text().toInt();

	m_pYKImageProc->m_iNumOfDispMarker = m_pParent->ui.lineEditNumOfMarkerDisp->text().toInt();
	m_pYKImageProc->m_iNumOfTrackMarker = m_pParent->ui.lineEditNumOfMarkerTrack->text().toInt();


	//m_arrYKImage[curIdx].m_iNumOfDispMarker = ui.lineEditNumOfMarkerDisp->text().toInt();
	//m_arrYKImage[curIdx].m_iNumOfTrackMarker = ui.lineEditNumOfMarkerTrack->text().toInt();	
	m_paramset.preGaussGamma = m_pParent->ui.lineEditPreGaussianGamma->text().toFloat();
	m_paramset.windUpper = m_pParent->ui.lineEditWindowFilteringUpperOffset->text().toInt();
	m_paramset.windLower = m_pParent->ui.lineEditWindowFilteringLowerOffset->text().toInt();
	m_paramset.derivateY_threPerc = m_pParent->ui.lineEditYDerivativeThre->text().toInt();
	m_paramset.derivateLine_threPerc = m_pParent->ui.lineEditYDerivativeLineThre->text().toInt();
	m_paramset.sobel_threPerc = m_pParent->ui.lineEditSobelThre->text().toInt();
	m_paramset.postGaussGamma = m_pParent->ui.lineEditPostGaussianGamma->text().toFloat();
	m_paramset.binary_threPerc = m_pParent->ui.lineEditBinaryThreshold->text().toInt();
	m_paramset.median_window = m_pParent->ui.lineEditMedianWindow->text().toInt();
	m_paramset.CC_overlap = m_pParent->ui.lineEditOverlapPix->text().toInt();
	m_fThreMultiplySD = m_pParent->ui.lineEditThresholdFactor->text().toDouble();
	m_iSameMarkerMargin = m_pParent->ui.lineEditMinDistBtwMarkers_mm->text().toDouble();
	m_iCandidate = m_pParent->ui.lineEditNumOfMarkerDisp->text().toInt();//later this should come from UI		
	m_motionW_X = m_pParent->ui.lineEditMotionWeightX->text().toDouble();
	m_motionW_Y = m_pParent->ui.lineEditMotionWeightY->text().toDouble();
	m_CC_weighting = m_pParent->ui.lineEditCC2mmConvF->text().toDouble(); //typical value 5.0	

	//int m_iMarginLt, m_iMarginTop, m_iMarginRt, m_iMarginBottom;
	m_strMargin = m_pParent->ui.lineEditROIMargin->text();
	m_strCommand = m_pParent->ui.lineEditCommandLine->text();

	m_vPrevFrameMarkerPosProc.clear();//no need but for sure
	m_vRefMarkerPosDCMProc.clear();

	//copy points //Should be modified later for Kalman Filter.More data sample is needed.
	//vector<QPoint> vPrevMarker = m_pParent->m_arrYKImage[m_iCrntIdx - 1].m_vMarker;
	/*if (m_iCrntIdx > 0)
	{
		vector<QPoint>::iterator it;
		for (it = vPrevMarker.begin(); it != vPrevMarker.end(); it++)
		{
			m_vPrevFrameMarkerPosProc.push_back(*it);
		}		
	}*/
	if (m_iCrntIdx > 0)
	{
		//find last kVOn marker			
		//int tmpCnt = m_iCrntIdx;

		for (int tmpIdx = m_iCrntIdx - 1; tmpIdx >= 0; tmpIdx--)
		{
			if (m_pParent->m_arrYKImage[tmpIdx].m_bKVOn)
			{
				m_vPrevFrameMarkerPosProc = m_pParent->m_arrYKImage[tmpIdx].m_vMarker;
				break;
			}
		}
		//m_vPrevFrameMarkerPosProc = m_pParent->m_arrYKImage[m_iCrntIdx - 1].m_vMarker;
	}

	/*vector<VEC3D>::iterator it3D;	
	for (it3D = m_pParent->m_vRefMarkerPosDCM.begin(); it3D != m_pParent->m_vRefMarkerPosDCM.end(); it3D++)
	{
		m_vRefMarkerPosDCMProc.push_back(*it3D);
	}	
*/
	m_vRefMarkerPosDCMProc = m_pParent->m_vRefMarkerPosDCM;

	cout << "copied m_pYKImageProc ROI width = " << m_pYKImageProc->m_rtROI.width() << endl;
}


//Ref position calculation
//perspective projection 2D from 3D
QPoint YKThreadProcImg::Get2DPosFromDCM3D(float DCM_X, float DCM_Y, float DCM_Z, float MV_GanAngle, float panelOffsetX, float panelOffsetY, float fSAD, float fSID, float width2D, float height2D, float spX, float spY)
{		
	//panelOffsetY = -panelOffsetY;//YKdebug: should be verified

	//From Excel file "Projection Image calc"
	//Sx_p = Source_x_prime

	//these positions are 3D and different from DICOM for Y
	//here, anterior is Y+ (while Y- in DICOM)
	VEC3D pos_Src;
	VEC3D pos_Panel;
	VEC3D pos_Src_p; //p means prime
	VEC3D pos_Panel_p; //p means prime
	VEC3D pos_A; //= DICOMX, -DICOMY, DICOMZ
	float scalar_t = 0.0;
	VEC3D pos_R;
	VEC3D pos_R_p;//after re-rotation
	VEC2D pos2D_Origin;
	int idxX, idxY; //final pixel index of panel (origin 0 0 : Top Left corner

	// minus: in rotation transformation, if direction is CW, angle should be minus
	pos_Src.fX = 0.0;
	pos_Src.fY = fSAD;//1000
	pos_Src.fZ = 0.0;

	pos_Panel.fX = 0.0;
	pos_Panel.fY = fSAD - fSID;//1536
	pos_Panel.fZ = 0.0;

	pos_A.fX = DCM_X;
	pos_A.fY = -DCM_Y;//*****************
	pos_A.fZ = DCM_Z;

	float radKV_GanAngle = -(MV_GanAngle + 90.0)*itk::Math::pi / 180.0;

	pos_Src_p.fX = cos(radKV_GanAngle)*pos_Src.fX - sin(radKV_GanAngle)*pos_Src.fY;
	pos_Src_p.fY = sin(radKV_GanAngle)*pos_Src.fX + cos(radKV_GanAngle)*pos_Src.fY;
	pos_Src_p.fZ = pos_Src.fZ;

	pos_Panel_p.fX = cos(radKV_GanAngle)*pos_Panel.fX - sin(radKV_GanAngle)*pos_Panel.fY;
	pos_Panel_p.fY = sin(radKV_GanAngle)*pos_Panel.fX + cos(radKV_GanAngle)*pos_Panel.fY;
	pos_Panel_p.fZ = pos_Panel.fZ;

	float tmp_upper = pos_Src_p.fX*(pos_Panel_p.fX - pos_Src_p.fX) + pos_Src_p.fY*(pos_Panel_p.fY - pos_Src_p.fY) + pos_Src_p.fZ*(pos_Panel_p.fZ - pos_Src_p.fZ);
	float tmp_lower = pos_Src_p.fX*(pos_A.fX - pos_Src_p.fX) + pos_Src_p.fY*(pos_A.fY - pos_Src_p.fY) + pos_Src_p.fZ*(pos_A.fZ - pos_Src_p.fZ);
	scalar_t = tmp_upper / tmp_lower;

	pos_R.fX = scalar_t*(pos_A.fX - pos_Src_p.fX) + pos_Src_p.fX;
	pos_R.fY = scalar_t*(pos_A.fY - pos_Src_p.fY) + pos_Src_p.fY;
	pos_R.fZ = scalar_t*(pos_A.fZ - pos_Src_p.fZ) + pos_Src_p.fZ;

	pos_R_p.fX = cos(-radKV_GanAngle)*pos_R.fX - sin(-radKV_GanAngle)*pos_R.fY;
	pos_R_p.fY = sin(-radKV_GanAngle)*pos_R.fX + cos(-radKV_GanAngle)*pos_R.fY;
	pos_R_p.fZ = pos_R.fZ;


	//pos2D_Origin.fX = -width2D / 2.0*spX + panelOffsetX;
	//pos2D_Origin.fY = height2D / 2.0*spY + panelOffsetY;
	//idxX = qRound((pos_R_p.fX - pos2D_Origin.fX) / spX);
	//idxY = qRound(-(pos_R_p.fZ - pos2D_Origin.fY) / spY); //in 3D, Z value is corresponding to Y in 2D

	//panelOffsetX, panelOffsetY: classic 
	pos2D_Origin.fX = -width2D / 2.0*spX + panelOffsetX;//e.g. -1024/2*0.4 +(-112.0) = -316.8 mm	
	pos2D_Origin.fY = -height2D / 2.0*spY - panelOffsetY; // DICOM position. in typical case, -204 - 10 = -214 mm (moved inferiorly)
	//panelOffsetY --> if this is PLUS --> panel is shifted INFERIORLY
	//in DICOM Z origin should go farther than no shift case


	idxX = qRound((pos_R_p.fX - pos2D_Origin.fX) / spX);
	idxY = qRound((pos_R_p.fZ - pos2D_Origin.fY) / spY); //DICOM Z coordinate, calculated from the left bottom
	idxY = height2D - idxY;//usually, index origin is left-top

	QPoint tmpPt;
	tmpPt.setX(idxX);
	tmpPt.setY(idxY);
	return tmpPt;
	//m_vRefMarkerPos2D; //Y direction is diff. from DCM. Updated from Timer event (or image update event acc. to gantry angle)
}



void YKThreadProcImg::ResetROIExt(YK16GrayImage* pYKImage, int marginLt, int marginTop, int marginRt, int marginBottom) //for current image only
{
	if (pYKImage == NULL)
		return;

	if (pYKImage->m_vMarkerRef.empty())
		return;

	vector<QPoint> vRefPoints = pYKImage->m_vMarkerRef;
	
	int full_width = pYKImage->m_iWidth;
	int full_height = pYKImage->m_iHeight;
	int ROI_Lt, ROI_Top, ROI_Rt, ROI_Bottom;

	int iMaxPosX = 0;
	int iMinPosX = pYKImage->m_iWidth - 1;

	int iMaxPosY = 0;
	int iMinPosY = pYKImage->m_iHeight - 1;

	if (vRefPoints.empty())
	{
		return;
	}
	else
	{
		vector<QPoint>::iterator it;
		for (it = vRefPoints.begin(); it != vRefPoints.end(); it++)
		{
			if ((*it).x() > iMaxPosX)
				iMaxPosX = (*it).x();
			if ((*it).x() < iMinPosX)
				iMinPosX = (*it).x();

			if ((*it).y() > iMaxPosY)
				iMaxPosY = (*it).y();
			if ((*it).y() < iMinPosY)
				iMinPosY = (*it).y();
		}
		ROI_Rt = iMaxPosX + marginRt;
		ROI_Bottom = iMaxPosY + marginBottom;
		ROI_Lt = iMinPosX - marginLt;
		ROI_Top = iMinPosY - marginTop;
	}

	if (ROI_Rt >= full_width) //not allowed
		ROI_Rt = full_width - 1;
	if (ROI_Lt < 0) //not allowed
		ROI_Lt = 0;
	if (ROI_Bottom >= full_height) //not allowed
		ROI_Bottom = full_height - 1;
	if (ROI_Top < 0) //not allowed
		ROI_Top = 0;
		
	pYKImage->setROI(ROI_Lt, ROI_Top, ROI_Rt, ROI_Bottom);	
}

void YKThreadProcImg::FillCurrentROI(YK16GrayImage* pYKImageFull, YK16GrayImage* pYKImageROI)
{
	if (pYKImageFull == NULL)
		return;

	if (pYKImageFull->m_rtROI.width() < 1 || pYKImageFull->m_rtROI.height() < 1)
	{
		cout << "ERROR! ROI is not set yet" << endl;
		return;
	}


	int iROILeft, iROITop, iROIRight, iROIBottom;
	iROILeft = pYKImageFull->m_rtROI.left();
	iROITop = pYKImageFull->m_rtROI.top();
	iROIRight = pYKImageFull->m_rtROI.right();
	iROIBottom = pYKImageFull->m_rtROI.bottom();

	//int ROIWidth = pYKImageFull->m_rtROI.width(); // right - left +1
	//int ROIHeight = pYKImageFull->m_rtROI.height();
	int ROIWidth = iROIRight - iROILeft; // right - left +1
	int ROIHeight = iROIBottom - iROITop;
	int fullWidth = pYKImageFull->m_iWidth;
	int fullHeight = pYKImageFull->m_iHeight;

	//int curIdx = ui.spinBoxImgIdx->value();
	//YK16GrayImage* pCurImg = &m_arrYKImage[curIdx];
	//offset = ROI left and top

	//pYKImageROI = new YK16GrayImage(ROIWidth, ROIHeight);//YKdebug!! why not working???
	pYKImageROI->CreateImage(ROIWidth, ROIHeight, 0);

	//Init itk ROI Buffer
	UnsignedShortImageType::IndexType start;
	start.Fill(0); //basic offset for iteration --> should be 0 if full size image should be processed

	UnsignedShortImageType::SizeType size;
	size[0] = ROIWidth;
	size[1] = ROIHeight;
	//size[0] = ROI_Rt - ROI_Lt+1;
	//size[1] = ROI_Bottom - ROI_Top+1;

	UnsignedShortImageType::RegionType region;
	region.SetSize(size);
	region.SetIndex(start);
	m_spSrcImage->SetRegions(region);
	m_spSrcImage->Allocate();


	int ROI_i = 0;
	for (int i = iROITop; i < iROIBottom; i++)
	{
		int ROI_j = 0;
		for (int j = iROILeft; j < iROIRight; j++)
		{
			pYKImageROI->m_pData[ROI_i*ROIWidth + ROI_j] = pYKImageFull->m_pData[fullWidth*i + j];
			ROI_j++;
		}
		ROI_i++;
	}
	pYKImageROI->SetSpacing(pYKImageFull->m_fSpacingX, pYKImageFull->m_fSpacingY);
	//m_bDisplayROI8Bit = false;

}

void YKThreadProcImg::FFT_NormalizedCrossCorrelation(YK16GrayImage* pYKImageROI, int iReqNumOverlapPix)
{
	if (pYKImageROI == NULL)
		return;

	QTime procedureStart = QTime::currentTime();

	if (pYKImageROI->m_iWidth != m_spSrcImage->GetBufferedRegion().GetSize()[0] ||
		pYKImageROI->m_iHeight != m_spSrcImage->GetBufferedRegion().GetSize()[1])
	{
		cout << "ROI image size is not matched btw. itk and YKImage." << endl;
		return;
	}

	YK16GrayImage::CopyYKImage2ItkImage(pYKImageROI, m_spSrcImage); //m_spSrcImage: relative src image --> everytime changes	

	typedef itk::FFTNormalizedCorrelationImageFilter<UnsignedShortImageType, FloatImageTypeYK> CorrelationFilterType;
	CorrelationFilterType::Pointer correlationFilter = CorrelationFilterType::New();

	correlationFilter->SetFixedImage(m_spSrcImage);
	correlationFilter->SetMovingImage(m_spCCTemplate);
	correlationFilter->SetRequiredNumberOfOverlappingPixels(iReqNumOverlapPix);
	correlationFilter->Update();	

        typedef itk::MinimumMaximumImageCalculator<FloatImageTypeYK> MinimumMaximumImageCalculatorType;
	MinimumMaximumImageCalculatorType::Pointer minimumMaximumImageCalculatorFilter = MinimumMaximumImageCalculatorType::New();
	minimumMaximumImageCalculatorFilter->SetImage(correlationFilter->GetOutput());
	minimumMaximumImageCalculatorFilter->Compute();

	itk::Index<2> maximumCorrelationPatchCenter = minimumMaximumImageCalculatorFilter->GetIndexOfMaximum();
	itk::Size<2> outputSize = correlationFilter->GetOutput()->GetLargestPossibleRegion().GetSize();
	itk::Index<2> maximumCorrelationPatchCenterFixed;

	maximumCorrelationPatchCenterFixed[0] = qRound(outputSize[0] / 2.0 - maximumCorrelationPatchCenter[0]);
	maximumCorrelationPatchCenterFixed[1] = qRound(outputSize[1] / 2.0 - maximumCorrelationPatchCenter[1]);

        typedef itk::CropImageFilter <FloatImageTypeYK, FloatImageTypeYK> CropImageFilterType;
        typedef itk::RescaleIntensityImageFilter<FloatImageTypeYK, UnsignedShortImageType> RescaleFilterType;
	RescaleFilterType::Pointer rescaleFilter = RescaleFilterType::New();

	//Crop image filter should be applied

        FloatImageTypeYK::SizeType sizeDiff;
	sizeDiff[0] = qRound((correlationFilter->GetOutput()->GetBufferedRegion().GetSize()[0] - m_pYKImageROIProc->m_iWidth) / 2.0);
	sizeDiff[1] = qRound((correlationFilter->GetOutput()->GetBufferedRegion().GetSize()[1] - m_pYKImageROIProc->m_iHeight) / 2.0);

	if (sizeDiff[0] > 0)
	{
		CropImageFilterType::Pointer CropFilter = CropImageFilterType::New();
		CropFilter->SetBoundaryCropSize(sizeDiff);
		CropFilter->SetInput(correlationFilter->GetOutput());
		CropFilter->Update();


		sizeDiff[0] = CropFilter->GetOutput()->GetBufferedRegion().GetSize()[0] - pYKImageROI->m_iWidth;
		sizeDiff[1] = CropFilter->GetOutput()->GetBufferedRegion().GetSize()[1] - pYKImageROI->m_iHeight;

		if (sizeDiff[0] != 0 || sizeDiff[1] != 0)
		{
			cout << "Cross-correlation error! template size is not matching ROI image even after cropping" << endl;
			return;
		}

		rescaleFilter->SetInput(CropFilter->GetOutput());
	}
	else if (sizeDiff[0] == 0 && sizeDiff[1] == 0)
	{
		//no need of cropping
		rescaleFilter->SetInput(correlationFilter->GetOutput());
	}
	else //negative value:
	{
		cout << "Cross-correlation error! template is bigger than source ROI image" << endl;
		return;
	}

	rescaleFilter->SetOutputMinimum(0);
	rescaleFilter->SetOutputMaximum(255); //CC value is no more 0 - 1
	rescaleFilter->Update();

	UnsignedShortImageType::Pointer spTarImage = rescaleFilter->GetOutput(); //no allocation required	

	//YKsdebug //if ROI size is different, the below func will not work
	YK16GrayImage::CopyItkImage2YKImage(spTarImage, pYKImageROI);

	//m_bDisplayROI8Bit = true;
	//printf("FFTNormalizedCorrelation Filter= %d\n", procedureStart.elapsed());

	pYKImageROI->m_bDraw8Bit = true;
}

void YKThreadProcImg::ImgProcessingForROI(YK16GrayImage* pYKImage, YK16GrayImage* pYKImageROI, IMGPROC_PARAMSET& stParamset)
{
	if (pYKImage == NULL)
		return;
	/*if (m_iROILeft < 0 ||
	m_iROITop < 0 ||
	m_iROIRight < 0 ||
	m_iROIBottom < 0)
	return;*/

	QTime procedureStart = QTime::currentTime();

	FillCurrentROI(pYKImage, pYKImageROI); //fill m_pImageYKCurROI //itk image as well

	//Read command line
	//QString strCommand = ui.lineEditCommandLine->text();
	QStringList strListCommand = m_strCommand.split(",");

	int commandSize = strListCommand.size();
	/*if (commandSize < 1)
	return;*/

	/*double gamma = 0.0;
	int threPercent = 0;
	int wndSize = 0;
	int iOverlapPix = 0;
*/
	for (int i = 0; i<commandSize; i++)
	{
		int commandIdx = strListCommand.at(i).toInt();

		switch (commandIdx)
		{
		case 0:
			//gamma = ui.lineEditPreGaussianGamma->text().toDouble();			
		//	GaussianSmoothing(stParamset.preGaussGamma);
			break;
		case 1:
			//SLT_IntensityWindowing();
			break;
		case 2:
			//SLT_FirstDerivative_Y();
			//threPercent = ui.lineEditYDerivativeThre->text().toInt();
			//FirstDerivative_Y(stParamset.derivateY_threPerc);
			break;
		case 3:
			//threPercent = ui.lineEditYDerivativeLineThre->text().toInt();
			//FirstDerivative_Y_Line(stParamset.derivateLine_threPerc);
			break;
		case 4:
			//threPercent = ui.lineEditSobelThre->text().toInt();
			//SobelEdgeDetection(stParamset.sobel_threPerc, pYKImageROI);
			break;
		case 5:
			//SLT_GaussianSmoothingPost();
			//gamma = ui.lineEditPostGaussianGamma->text().toDouble();
			//GaussianSmoothing(stParamset.postGaussGamma);
			break;
		case 6:
			//threPercent = ui.lineEditBinaryThreshold->text().toInt(); //should be modified
			//BinaryImage(stParamset.binary_threPerc);
			break;
		case 7:
			//SLT_MedianFiltering();
			//wndSize = ui.lineEditMedianWindow->text().toInt();
			//MedianFiltering(stParamset.median_window);
			break;
		case 8:
			//SLT_FFTNormCC();
			//iOverlapPix = ui.lineEditOverlapPix->text().toInt();
			FFT_NormalizedCrossCorrelation(pYKImageROI, stParamset.CC_overlap);			
			//FindMarkerInROI_A(); //result: m_vMarkerPosPre w/ sorting
			//SLT_DrawProjImage();
		}
	}

	//ui.lineEditTimePerProcessing->setText(QString("%1").arg(procedureStart.elapsed()));

	return;
	/* Image Processing End */
}



void YKThreadProcImg::FindMarkerInROI_A(YK16GrayImage* pImageYKFull, YK16GrayImage* pImageYKCurROI, vector<PIXINFO>& vOutputMarkerWithCC, float fThreMultiplySD, float sameMarkerMargin)
{
	if (pImageYKFull == NULL)
		return;

	if (pImageYKCurROI == NULL)
		return;

	//float fThreMultiplySD = ui.lineEditThresholdFactor->text().toDouble();
	//float sameMarkerMargin = ui.lineEditMinDistBtwMarkers_mm->text().toDouble();
	float pxDistThre = sameMarkerMargin*1.536 / pImageYKCurROI->m_fSpacingX; //15.36/0.4 = 40 px

	//Assume that it is ranging from 0 - 255
	//1) Find a threshold
	pImageYKCurROI->CalcImageInfo();
	float fCCThre = pImageYKCurROI->m_fPixelMean + fThreMultiplySD * pImageYKCurROI->m_fPixelSD; // if norm distribution is assumed, only 5% of pixels are considered

	//if the SD is too big
	if (fCCThre >= pImageYKCurROI->m_fPixelMax)
		fCCThre = pImageYKCurROI->m_fPixelMax * 0.7;

	//PIXINFO
	vector<PIXINFO> vHighCCPt;
	int imgSize = pImageYKCurROI->m_iWidth*pImageYKCurROI->m_iHeight;

	int width = pImageYKCurROI->m_iWidth;
	int height = pImageYKCurROI->m_iHeight;

	unsigned short tmpVal;
	PIXINFO stTmpPixInfo;

	for (int i = 0; i < height; i++)
	{
		for (int j = 0; j < width; j++)
		{
			tmpVal = (float)(pImageYKCurROI->m_pData[i*width + j]);
			if (tmpVal > fCCThre)
			{
				stTmpPixInfo.posX = j;
				stTmpPixInfo.posY = i;
				stTmpPixInfo.val = tmpVal;
				vHighCCPt.push_back(stTmpPixInfo);
			}
		}
	}

	//Sort them
	sort(vHighCCPt.begin(), vHighCCPt.end(), ComparePixInfoProc);

	//cout << vHighCCPt.at(0).val << " " << vHighCCPt.at(1).val << endl;
	//Delete duplicate (points around the same position already detected)
	//matlab code



	//Update the marker position to current "full" image
	//int crntIdx = ui.spinBoxImgIdx->value();

	vector<PIXINFO> vTmpMarkerWithCC;
	//vOutputMarker.clear(); //registered marker position //should be local



	vector<PIXINFO>::iterator it_all, it_reg;
	float tmpDist = 0.0;

	bool bRedundant = false;

	for (it_all = vHighCCPt.begin(); it_all != vHighCCPt.end(); it_all++)
	{
		bRedundant = false;

		for (it_reg = vTmpMarkerWithCC.begin(); it_reg != vTmpMarkerWithCC.end(); it_reg++)
		{
			tmpDist = sqrt(pow((*it_all).posX - (*it_reg).posX, 2.0) + pow((*it_all).posY - (*it_reg).posY, 2.0));

			if (tmpDist < pxDistThre)
			{
				bRedundant = true;
				if ((*it_all).val >(*it_reg).val)
				{
					(*it_reg) = (*it_all);
				}
			}
		}

		if (!bRedundant)
			vTmpMarkerWithCC.push_back(*it_all);
	}

	sort(vTmpMarkerWithCC.begin(), vTmpMarkerWithCC.end(), ComparePixInfoProc);


	pImageYKFull->ClearMarkerPos();
	int ROI_start_x = pImageYKFull->m_rtROI.left();
	int ROI_start_y = pImageYKFull->m_rtROI.top();

	PIXINFO tmpPt;
	int boundaryMarginX = 10; //10px
	int boundaryMarginY = 10;

	vOutputMarkerWithCC.clear();
	int iTmpCnt = 0;
	//int iMaxRegiMarker = pImageYKFull->m_iNumOfDispMarker;
	for (it_reg = vTmpMarkerWithCC.begin(); it_reg != vTmpMarkerWithCC.end(); it_reg++)
	{
		tmpPt = *it_reg;

		//exclude boundary high CC points
		if (tmpPt.posX > boundaryMarginX && tmpPt.posX < pImageYKFull->m_rtROI.width() - boundaryMarginX
			&& tmpPt.posY > boundaryMarginY && tmpPt.posY < pImageYKFull->m_rtROI.height() - boundaryMarginY)
		{
			if (iTmpCnt >= pImageYKFull->m_iNumOfDispMarker)
				break;

			pImageYKFull->AddMarkerPos(ROI_start_x + (*it_reg).posX, ROI_start_y + (*it_reg).posY);
			vOutputMarkerWithCC.push_back(tmpPt);
			iTmpCnt++;
		}
	}
}

//REORDER THE m_arrYKImage[crntIdx].m_vMarker
double YKThreadProcImg::FindMarkerInROI_B(YK16GrayImage* pImageYKFull, YK16GrayImage* pImageYKCurROI,
	vector<PIXINFO>& vInputMarkerWithCC, int iCandidate, int iNumOfTrackMarker,
	double motionW_X, double motionW_Y, double CC_weighting, vector<QPoint>& vPrevFrameMarkerPos)
{
	//vInputMarker is also needed to use CC value penalty!
	if (pImageYKFull == NULL)
		return -1.0;

	if (pImageYKCurROI == NULL)
		return -1.0;
	
	//audit current detected markers by using prior information of markers from CT
	//Compare m_arrYKImage[0].m_vMarkerRef (1,2,3 ordered) to //m_arrYKImage[0].m_vMarker

	//Final Goal:
	//currently, m_arrYKImage[crntIdx].m_vMarker is already containing marker positions. These markersd are reordered by the auditing based on m_vMarkerRef

	//Since there are some wrongly detected points in vertabrae body region, motion continuity term has been added		

	//double motionW_X = ui.lineEditMotionWeightX->text().toDouble();
	//double motionW_Y = ui.lineEditMotionWeightY->text().toDouble();
	//double CC_weighting = ui.lineEditCC2mmConvF->text().toDouble(); //typical value 5.0	

	vector<QPoint> vMarkerPosRef;
	vector<QPoint> vMarkerPosComp;
	vector<QPoint> vInRangeMarkerPosRef;//w/o outof range ref points in 2D:  original ref size - cnt of true values of vIsOutOfRange	
	vector<bool> vIsOutOfRange; //same size as original ref

	vMarkerPosComp = pImageYKFull->m_vMarker;//all candidates
	vMarkerPosRef = pImageYKFull->m_vMarkerRef;
	vIsOutOfRange = pImageYKFull->m_bvRefOutOfRange;

	//1) Find optimal combinations		
	//int cntRef = pImageYKFull->m_vMarkerRef.size();	
	int iNumOfRefMarker = pImageYKFull->m_vMarkerRef.size();

	if (vMarkerPosRef.empty())
	{
		cout << "No reference maker positions. Prior information cannot be used." << endl;
		return -1.0;
	}

	if (vMarkerPosComp.empty())
	{
		cout << "Error! No maker positions were found from Find_A function. " << endl;
		return -1.0;
	}

	//int iCandidate = ui.lineEditNumOfMarkerDisp->text().toInt();//later this should come from UI	
	//int iNumOfTrackMarker = ui.lineEditNumOfMarkerTrack->text().toInt();

	//YKdebug: if there is migrations, this can happen
	//in this case, this kind of auditing can not be used
	if (iNumOfRefMarker != iNumOfTrackMarker)
	{
		cout << "To use this filter, number of tracking markers and reference marker should be matched" << endl;
		return -1.0;
	}

	if (iCandidate >= vMarkerPosComp.size())
		iCandidate = vMarkerPosComp.size();

	vector<bool>::iterator itOutRange;
	vector<QPoint>::iterator itRefPt;

	for (itOutRange = vIsOutOfRange.begin(), itRefPt = vMarkerPosRef.begin(); itOutRange != vIsOutOfRange.end(), itRefPt != vMarkerPosRef.end(); itOutRange++, itRefPt++)
	{
		if (!(*itOutRange))
			vInRangeMarkerPosRef.push_back(*itRefPt);
	}

	iNumOfRefMarker = vInRangeMarkerPosRef.size();

	vector<int> vIdx;
	vector<int>::iterator it;

	for (int i = 0; i < iCandidate; i++)
	{
		vIdx.push_back(i);
	}
	vector<int> vCombinOutput;
	Combination(vIdx, iNumOfRefMarker, vCombinOutput);
	//if 5C3 =10 --> 30 items in voutput e.g. 2 3 4 ... 0 1 2...

	vector<int> vCurrentTestIndex;	
	vector<int> vBestPointIndex;

	int iCombCnt = qRound(vCombinOutput.size() / (double)iNumOfRefMarker);	//10 in this case

	double minScoreSum = 999999.0;
	double VecErrorOfMinScore = 0.0;
	double WeightedPosErrorOfMinScore = 0.0;
	double CCPenaltyOfMinScore = 0.0;

	double curDiffVecSum = 0.0;
	double weightedPosDiff = 0.0;
	double CC_penalty = 0.0;

	double curScoreSum = 0.0;
	double tmpCCSum = 0.0;	


	for (int i = 0; i < iCombCnt; i++)
	{
		vCurrentTestIndex.clear();

		for (int j = 0; j < iNumOfRefMarker; j++)
		{
			vCurrentTestIndex.push_back(vCombinOutput.at(i*iNumOfRefMarker + j));
		}

		//for each combination		
		//vCurrentTestIndex //permutation here for 3 points
		//Proir1: Relative position of each marker
		curDiffVecSum = ComputeMinVecDiffAndSortIndex(vCurrentTestIndex, vInRangeMarkerPosRef, vMarkerPosComp);


		//Proir2: MOTION 
		float errorSumX = 0.0; float errorSumY = 0.0;

		//vector<QPoint> vPrevFrameMarker = m_arrYKImage[crntIdx - 1].m_vMarker;

		if (!vPrevFrameMarkerPos.empty())
		{
			int cnt = 0;
			if (vPrevFrameMarkerPos.size() == vCurrentTestIndex.size())
			{
				for (it = vCurrentTestIndex.begin(); it != vCurrentTestIndex.end(); it++)
				{
					errorSumX = errorSumX + pow((double)(vMarkerPosComp.at(*it).x() - vPrevFrameMarkerPos.at(cnt).x()), 2.0);
					errorSumY = errorSumY + pow((double)(vMarkerPosComp.at(*it).y() - vPrevFrameMarkerPos.at(cnt).y()), 2.0);
					cnt++;
				}
				weightedPosDiff = motionW_X*sqrt(errorSumX) + motionW_Y*sqrt(errorSumY);
			}
			else
			{
				weightedPosDiff = 0.0;
			}
		}
		else
			weightedPosDiff = 0.0;

		//Bring the previouis frame info

		/*meanPosPrevX = (double)m_arrYKImage[crntIdx - 1].GetMeanPos().x();
		meanPosPrevY = (double)m_arrYKImage[crntIdx - 1].GetMeanPos().y();
		weightedPosDiff = wtLatAntPost * fabs(meanPosX - meanPosPrevX) + wtSupInf * fabs(meanPosY - meanPosPrevY);*/

		//Means position doesn't work well.
		//Let's use RMSE for each marker	

		//Proir3: CC_value penanlty
		//CC value: 0 ~ 1

		//vMarkerPosComp and 

		if (vInputMarkerWithCC.size() == vMarkerPosComp.size()) //should be same!
		{
			//CC_SUM_penalty
			int cnt = 0;
			tmpCCSum = 0.0;
			for (it = vCurrentTestIndex.begin(); it != vCurrentTestIndex.end(); it++)
			{
				tmpCCSum = tmpCCSum + vInputMarkerWithCC.at(*it).val;
				cnt++;
			}
			CC_penalty = (1 - tmpCCSum / (double)cnt / 256.0)*CC_weighting; //1-0.3 = 0.7
		}
		else
		{
			cout << "ERROR! NOT INTENDED. CHECK FindMarkerInROI_B" << endl;
		}

		//ENERGY FUNCTION (Score)
		curScoreSum = curDiffVecSum + weightedPosDiff + CC_penalty; //lower the better


		if (minScoreSum >= curScoreSum)
		{
			minScoreSum = curScoreSum;
			VecErrorOfMinScore = curDiffVecSum;
			WeightedPosErrorOfMinScore = weightedPosDiff;
			CCPenaltyOfMinScore = CC_penalty;

			vBestPointIndex.clear();

			for (int j = 0; j < iNumOfRefMarker; j++)
			{
				vBestPointIndex.push_back(vCurrentTestIndex.at(j)); //BestPoint index order is corresponding to the reference one
			}
		}
	}

	//double meanDistanceToRef = minDiffVecSum / (double)iNumOfRefMarker; //pixels //uncertainty
	

	//ui.lineEditErrorPriorInfo->setText(QString::number(meanOverallDistanceScore, 'f', 2));

	//if This is beyond some criteria, reject this tracking

	//vBestPointIndex: e.g. 1,3,4 --> among all the dtectected poitns, 1,3,4 markers maeks the best matching with CT fef. Not sorted.
	//Permutation in 

	//chose n of "cntRef" index to calculate some error value
	//if ref n is 3, choose 3 indexes among all candidates
	//0,1,2 0,1,3 0,1,4

	//Replace the candidate points with final detected ones

	pImageYKFull->m_track_priorErr = VecErrorOfMinScore;
	pImageYKFull->m_track_motionErr = WeightedPosErrorOfMinScore;
	pImageYKFull->m_track_CC_penalty = CCPenaltyOfMinScore;
	

	vector<QPoint> vTmpMarkerFinal;

	vector<int>::iterator itIdx;
	for (itIdx = vBestPointIndex.begin(); itIdx != vBestPointIndex.end(); itIdx++)
	{
		vTmpMarkerFinal.push_back(vMarkerPosComp.at(*itIdx));
	}

	//Finally update FullImage->MarkerPos here. Updating vMarkerPosComp is not working!!

	//vMarkerPosComp.clear();
	pImageYKFull->m_vMarker.clear();

	vector<bool>::iterator itIsOutOfRange = vIsOutOfRange.begin();
	for (int i = 0; i < vBestPointIndex.size(); i++) //should be less than size of vTmpMarkerFinal
	{
		if (*itIsOutOfRange)
			pImageYKFull->m_vMarker.push_back(vMarkerPosRef.at(i)); //if the corresponding Ref is out of Range, just copy Ref one. it can anyway be detected by YK16Image::m_bvOutofRange
		else
			pImageYKFull->m_vMarker.push_back(vTmpMarkerFinal.at(i));

		itIsOutOfRange++;
	}

	double meanOverallDistanceScore = minScoreSum / (double)iNumOfRefMarker; //pixels //uncertainty //return value!!!!
	return meanOverallDistanceScore;
}



// vOutputComb is a 1D vector consisting of all combinations
void YKThreadProcImg::Combination(std::vector<int>& vIndex, int combCnt, std::vector<int>& vOutputComb)
{
	if (!vOutputComb.empty())
		vOutputComb.clear();

	if (combCnt > vIndex.size())
		return;

	std::vector<bool> bitset(vIndex.size() - combCnt, 0);
	bitset.resize(vIndex.size(), 1);

	do
	{
		for (std::size_t i = 0; i != vIndex.size(); ++i)
		{
			if (bitset[i])
			{
				vOutputComb.push_back(vIndex[i]);
			}
		}
	} while (std::next_permutation(bitset.begin(), bitset.end()));
}



double YKThreadProcImg::ComputeMinVecDiffAndSortIndex(vector<int>& vCurCompIndex, vector<QPoint>& vMarkerPosRef, vector<QPoint>& vMarkerPosComp)
{
	if (vCurCompIndex.empty())
		return -1.0;

	int cntMarker = vCurCompIndex.size();

	if (cntMarker != vMarkerPosRef.size())
		return -1.0;

	if (cntMarker > vMarkerPosComp.size())
		return -1.0;

	vector<QPoint> vSubMarkerPosComp;
	vector<int> vBestIndex;
	QPoint tmpPt;

	double minVecDiff = 999999.9;
	double curVecDiff = 0.0;

	std::sort(vCurCompIndex.begin(), vCurCompIndex.end());
	do {
		vSubMarkerPosComp.clear();
		for (int i = 0; i < cntMarker; i++)
		{
			tmpPt = vMarkerPosComp.at(vCurCompIndex.at(i));
			vSubMarkerPosComp.push_back(tmpPt);
		}
		curVecDiff = GetMeanVecDiff(vMarkerPosRef, vSubMarkerPosComp);
		if (curVecDiff >= 0.0 && curVecDiff < minVecDiff)
		{
			minVecDiff = curVecDiff;

			vBestIndex.clear();
			for (int i = 0; i < cntMarker; i++)
			{
				vBestIndex.push_back(vCurCompIndex.at(i));
			}
		}
	} while (std::next_permutation(vCurCompIndex.begin(), vCurCompIndex.end()));


	for (int i = 0; i < cntMarker; i++)
	{
		vCurCompIndex.at(i) = vBestIndex.at(i);
	}
	return minVecDiff;
}


double YKThreadProcImg::GetMeanVecDiff(vector<QPoint>& vMarkerPosRef, vector<QPoint>&vMarkerPosComp)
{
	//size should be same
	if (vMarkerPosRef.empty())
		return -1.0;
	int iCntSize = vMarkerPosRef.size();
	if (iCntSize != vMarkerPosComp.size())
		return -1.0;

	//Calculate the COM
	float fCOM_ref_x = 0.0;
	float fCOM_ref_y = 0.0;

	float fCOM_comp_x = 0.0;
	float fCOM_comp_y = 0.0;

	vector<QPoint>::iterator itRef, itComp;

	for (itRef = vMarkerPosRef.begin(); itRef != vMarkerPosRef.end(); itRef++)
	{
		fCOM_ref_x = fCOM_ref_x + (*itRef).x();
		fCOM_ref_y = fCOM_ref_y + (*itRef).y();
	}
	fCOM_ref_x = fCOM_ref_x / (float)iCntSize;
	fCOM_ref_y = fCOM_ref_y / (float)iCntSize;

	for (itComp = vMarkerPosComp.begin(); itComp != vMarkerPosComp.end(); itComp++)
	{
		fCOM_comp_x = fCOM_comp_x + (*itComp).x();
		fCOM_comp_y = fCOM_comp_y + (*itComp).y();
	}
	fCOM_comp_x = fCOM_comp_x / (float)iCntSize;
	fCOM_comp_y = fCOM_comp_y / (float)iCntSize;

	float sumOfAbsDiff = 0.0;
	VEC2D tmpVectorRef;
	VEC2D tmpVectorComp;
	VEC2D tmpVectorDiff;

	for (itRef = vMarkerPosRef.begin(), itComp = vMarkerPosComp.begin(); itRef != vMarkerPosRef.end() && itComp != vMarkerPosComp.end(); itRef++, itComp++)
	{
		tmpVectorRef.fX = (*itRef).x() - fCOM_ref_x;
		tmpVectorRef.fY = (*itRef).y() - fCOM_ref_y;

		tmpVectorComp.fX = (*itComp).x() - fCOM_comp_x;
		tmpVectorComp.fY = (*itComp).y() - fCOM_comp_y;

		tmpVectorDiff.fX = tmpVectorComp.fX - tmpVectorRef.fX;
		tmpVectorDiff.fY = tmpVectorComp.fY - tmpVectorRef.fY;

		sumOfAbsDiff = sumOfAbsDiff + sqrt(pow((double)tmpVectorDiff.fX, 2.0) + pow((double)tmpVectorDiff.fY, 2.0));
	}
	return sumOfAbsDiff / (double)iCntSize;
}
